#include "MotionSegmenter.h"

//#define DUMP_VAL

#define GENERIC -1
#define LOCAL_MINIMA 0
#define LOCAL_MAXIMA 1
#define SADDLE_POINT 2
#define FLAT 3


#define ABS(a)  (((a) < 0) ? -(a) : (a))

#define SIGN(a)  ((a < 0) ? -1 : ((a > 0) ? 1 : 0))


MotionSegmenter::MotionSegmenter(std::vector<CGMesh*>& meshlist, int active, GLCamera& camera, QWidget* parent)
	: QWidget(parent), 
	_meshlist(meshlist),
	_camera(camera)
{
	
	srand(time(NULL));

	//create the window
	this->setMinimumSize(300,100);
	this->setMaximumSize(500,500);
	this->setWindowTitle("Motion Segmentation");
    this->setWindowIcon(QIcon(tr("logo.ico")));

    _quality = maxq;			//variance is default measure
	_active = active;
	_silhouetteCounter = 0;
	srand(time(NULL));

	//create layout
	QGridLayout* _layout = new QGridLayout();
	//PUSH BUTTONS
	QPushButton* _getSilhouettesGPUButton = new QPushButton("A.Silhouettes Extraction", this);
	_dataCollect = new QPushButton("A.Silhouettes Analysis", this);
	_clusterDataButton = new QPushButton("Clustering step", this);
	QPushButton* _finalize = new QPushButton("Finalize", this);
	QPushButton* _saveSegmentation = new QPushButton("Export results", this);
    QPushButton* _openSegmentation = new QPushButton("Open Project...", this);

	this->setFocusPolicy(Qt::NoFocus);

	//spinboxes
	_vpSpinBox = new QSpinBox(this);						//spin box for the number of viewpoints
	_vpSpinBox->setMinimum(1);
	_vpSpinBox->setMaximum(25);
	_vpSpinBox->setSingleStep(1);
	_vpSpinBox->setValue(25);

	_ksSpinBox = new QSpinBox(this);
	_ksSpinBox->setMinimum(1);
	_ksSpinBox->setMaximum(250);
	_ksSpinBox->setSingleStep(1);
	_ksSpinBox->setValue(50);

	//LABELS
	_silhouetteLabelGPU = new QLabel("Captured: 0", this);

	//RADIO BUTTONS
	_qualityMeasureButtonGroup = new QButtonGroup(this);

	QRadioButton* _meanRadioButton = new QRadioButton("mean", this);
	QRadioButton* _maxRadioButton = new QRadioButton("max", this);
	QRadioButton* _varRadioButton = new QRadioButton("var", this);

	_qualityMeasureButtonGroup->addButton(_meanRadioButton);
	_qualityMeasureButtonGroup->addButton(_maxRadioButton);
	_qualityMeasureButtonGroup->addButton(_varRadioButton);

	_maxRadioButton->setChecked(true);	//default


	//add object to layout
	_layout->addWidget(_getSilhouettesGPUButton, 0, 0, 1, 4);
	_layout->addWidget(_vpSpinBox, 0, 4, 1, 1);
	_layout->addWidget(_ksSpinBox, 0, 5, 1, 1);
	_layout->addWidget(_silhouetteLabelGPU, 0, 6, 1, 2);
    _layout->addWidget(_meanRadioButton, 1, 1, 1, 2);
    _layout->addWidget(_maxRadioButton, 1, 3, 1, 2);
    _layout->addWidget(_varRadioButton, 1, 5, 1, 2);
    _layout->addWidget(_dataCollect, 2, 0, 1, 8);
    _layout->addWidget(_clusterDataButton, 3, 0, 1, 8);
    _layout->addWidget(_finalize, 4, 0, 1, 2);
    _layout->addWidget(_saveSegmentation, 4, 2, 1, 3);
    _layout->addWidget(_openSegmentation, 4, 5, 1, 3);
	
	extractionTime = 0.0f;
	analysisTime = 0.0f;
	clusteringTime = 0.0f;

	_clusterDataButton->setEnabled(false);
    //_alphaSlider->setEnabled(false);
	
	this->setLayout( _layout );

	ColorPicker* cp = new ColorPicker();
	cp->hide();

	//create connections
        connect ( _getSilhouettesGPUButton, SIGNAL(clicked()),          this,		SLOT(firstStep())               );
        connect ( _meanRadioButton,         SIGNAL(toggled(bool)),      this,		SLOT(setQualityMeasure())       );
        connect ( _maxRadioButton,          SIGNAL(toggled(bool)),      this,		SLOT(setQualityMeasure())       );
        connect ( _varRadioButton,          SIGNAL(toggled(bool)),      this,		SLOT(setQualityMeasure())       );
        connect ( _dataCollect,             SIGNAL(clicked()),          this,		SLOT(secondStep())              );
        connect ( _vpSpinBox,               SIGNAL(valueChanged(int)),	this,		SLOT(selectViewPoints(int))     );
        connect ( _ksSpinBox,               SIGNAL(valueChanged(int)),	this,		SLOT(selectSilhouettes(int))	);
        connect ( _clusterDataButton,       SIGNAL(clicked()),          this,		SLOT(thirdStep())	);
        connect ( _finalize,                SIGNAL(clicked()),          this,		SLOT(finalize())                );
        connect ( _saveSegmentation,        SIGNAL(clicked()),          this,           SLOT(saveSegmentation())        );
        connect ( _openSegmentation,        SIGNAL(clicked()),          this,           SLOT(openSegmentationSumner())        );
        connect ( cp,                       SIGNAL(sendColor(QColor)),	this,		SLOT(storeCluster(QColor))      );
        connect ( this,                     SIGNAL(showColorPicker()),	cp,             SLOT(show())                    );
        connect ( cp,                       SIGNAL(sendColor(QColor)),	this,		SLOT(colorLastCluster(QColor))	);
        connect ( cp,                       SIGNAL(removeCluster()),	this,		SLOT(undoLastCluster())         );
        connect ( cp,                       SIGNAL(mergeCluster()),     this,		SLOT(splitLastCluster())        );

	//initialization
	
	//no mesh to initialize camera position, exit
	if ( _meshlist.empty() ) return;
	//init camera position
	if ( _camPosition.empty() ) initCamPosition();
	selectSilhouettes( _ksSpinBox->value() );

	//init edges information
	if ( _edgesData.empty() ) initEdges(*_meshlist[_active]);
	//init CUDA informations
	collectCudaInformations();
}



void MotionSegmenter::setQualityMeasure() {
	int i = _qualityMeasureButtonGroup->checkedId();
	QRadioButton* tmp = (QRadioButton*) _qualityMeasureButtonGroup->button(i);

        if ( tmp->text().compare(tr("max")) == 0 ) { _quality = maxq; return; }
        if ( tmp->text().compare(tr("mean")) == 0) { _quality = mean; return; }
        if ( tmp->text().compare(tr("var")) == 0) { _quality = variance; return; }

	assert(0);
	return;
}


/**
 * Initialize positions of the camera, using a sphere uniformly sampled.
 **/
int MotionSegmenter::initCamPosition( ) {

	//create unit sphere
	CGMesh* mesh = new CGMesh();
	vcg::tri::Sphere(*mesh,1);		//factor 3 -> 642 vertices

	double tgtradius = _meshlist[_active]->bbox.Diag();

	_camPosition.clear();

	for ( int i = 0; i < mesh->vn; i ++ ) {
		if ( mesh->vert[i].P().Y() >= 0 )
			_camPosition.push_back(mesh->vert[i].P()*tgtradius); 
	}

	return _camPosition.size();
}

/** 
 * Init the edges data, creating the underlying structure.
 **/
int MotionSegmenter::initEdges( CGMesh& ref ) {
	//space reserve
	_edgesData.resize(3 * ref.fn);
	_edgesRef.resize(ref.fn);
	//update topology
	vcg::tri::UpdateTopology<CGMesh>::FaceFace(ref);
	//clean mark flags
	vcg::tri::UnMarkAll(ref);
	int edgecnt = 0;
	//loop on the faces
	for ( CGMesh::FaceIterator fit = ref.face.begin(); fit != ref.face.end(); ++ fit ) {
		//create node for each face
		lemon::ListGraph::Node s = dualGraph.addNode();
		assert( dualGraph.id(s) == vcg::tri::Index(ref, &(*fit)) );

		//loop around the neighbors
		for ( int i = 0; i < 3; i ++ ) {
			if ( vcg::tri::IsMarked( ref, (*fit).FFp(i) ) ) continue;	//already visited
			//...not visited! 
			//create the edge
			_edgesData[edgecnt].v0 = vcg::tri::Index( ref, (*fit).V(i) );
			_edgesData[edgecnt].v1 = vcg::tri::Index( ref, (*fit).V1(i) );
			_edgesData[edgecnt].f0 = vcg::tri::Index( ref, &(*fit) );
			_edgesData[edgecnt].f1 = vcg::tri::Index( ref, (*fit).FFp(i) );
			//update reference to the faces
			_edgesRef[_edgesData[edgecnt].f0].edges[i] = edgecnt;
			_edgesRef[_edgesData[edgecnt].f1].edges[(*fit).FFi(i)] = edgecnt;
			//increment counter
			edgecnt++;
		}

		//mark the face as visited
		vcg::tri::Mark( ref, &(*fit) );
	}

	_edgesData.resize(edgecnt);

	
	//create mesh-graph representation
	//create vertex
	for ( CGMesh::VertexIterator vit = ref.vert.begin(); vit != ref.vert.end(); ++ vit ) {
		lemon::ListGraph::Node s = meshGraph.addNode();
		assert( meshGraph.id(s) == vcg::tri::Index(ref, &(*vit)) );		//sanity check on index
	}

	//add connectivity to mesh graph and dual graph
	for ( int i = 0; i < _edgesData.size(); i ++ ) {
		//dual graph: connect faces
		lemon::ListGraph::Node s = dualGraph.nodeFromId( _edgesData[i].f0 );
		lemon::ListGraph::Node t = dualGraph.nodeFromId( _edgesData[i].f1 ); 
		lemon::ListGraph::Edge e = dualGraph.addEdge(s, t);
		assert( dualGraph.id(e) == i );

		//mesh graph: connect vertices
		lemon::ListGraph::Node v0 = meshGraph.nodeFromId(  _edgesData[i].v0 );
		lemon::ListGraph::Node v1 = meshGraph.nodeFromId(  _edgesData[i].v1 );
		lemon::ListGraph::Edge e0 = meshGraph.addEdge(v0, v1);
		assert( meshGraph.id(e0) == i );
	}
	qDebug() << "MeshGraph ok";

	updateEdgeLengths();

    //laplaceOperator();

	return edgecnt;
}



/**
 * Given and edge and one of its vertices, loop around the vertex and find adjacent silhouette edges on active mesh.
 * Return the indices of the adjacent edges.
 **/
std::vector<int> MotionSegmenter::getSilhouetteNeighbors( Edge _edge, int v ) {

	std::vector<int> output;

	//store pointers
	CGMesh::FaceType* face = &(_meshlist[_active]->face[_edge.f0]);
	CGMesh::VertexType* vert = &(_meshlist[_active]->vert[v]);
	//find index of the edge in f0
	int e; for ( e = 0; e < 3 && face->V(e) != vert; e ++ ); 
	if ( v == _edge.v1 ) e = (e+2)%3;		//if we are looping around v1, the index must be decreased

	vcg::face::Pos<CGMesh::FaceType> pos( face, e, vert );

	//loop!
	do {
		pos.FlipE();
		pos.FlipF();
		//avoid considering the current edge
		if ( pos.F() != face ) {
			//if we used cuda, use the values already stored
			if ( silhouetteValue[_edgesRef[vcg::tri::Index(*_meshlist[_active], pos.F())].edges[pos.E()]] ) {
					output.push_back( _edgesRef[vcg::tri::Index(*_meshlist[_active], pos.F())].edges[pos.E()] );
			}
		}
	} 
	while( pos.F() != face );
	
	return output;
}

void MotionSegmenter::firstStep() {

	qDebug() << "Extracting ASilhouettes...";

	if ( _meshlist.empty() ) return; 

	//clear the tree
	tree.clear();

	//if we haven't initialized the camera positions, do it
	if ( _camPosition.empty() ) initCamPosition();
	//if we havent' initialized the edges information, do it
	if ( _edgesData.empty() ) initEdges(*_meshlist[_active]);

	//_contourVector.resize(50);
	_silhouetteCounter = 0;		//start counter from 0

	//save current observer and active mesh
	CGPoint base = _camera.getObserver();
	int active = _active;
	
	time_t start = clock();

	//GPU allocation!
	float3* faceNormalsGPU;
	size_t faceNormalsSize = _meshlist[_active]->face.size() * sizeof(float3);
	cudaMalloc( (void**) &faceNormalsGPU, faceNormalsSize );		//allocate data containing normal informations

	float3* facePointsGPU;
	size_t facePointsSize = _meshlist[_active]->face.size() * sizeof(float3);
	cudaMalloc( (void**) &facePointsGPU, facePointsSize );		//allocate data containing points informations

	int2* edgeFacesGPU;
	size_t edgeFacesSize = _edgesData.size() * sizeof(int2);
	cudaMalloc( (void**) &edgeFacesGPU, edgeFacesSize );		//allocate data containing faces information from edge
	
	//Now allocate space for the output on the GPU
	bool* silhouetteValueGPU;
	size_t silhouetteValueSize = _edgesData.size() * sizeof(bool);
	cudaMalloc( (void**) &silhouetteValueGPU, silhouetteValueSize );

	//time keepers
	clock_t silhouette = 0;
	clock_t graph = 0;

    //place here the random selection
    _silhouetteVector.resize( silhouettePairs.size() );

	values.resize(_silhouetteVector.size());

    srand(time(NULL));

	//captures silhouettes
    _silhouetteCounter = 0;
	for ( std::set< std::pair<int,int> >::iterator sit = silhouettePairs.begin(); sit != silhouettePairs.end(); ++sit ) {
		_camera.setObserver( _camPosition[(*sit).first] );
		_active = (*sit).second;
		_silhouetteVector[_silhouetteCounter] = new ASilhouette( _meshlist, (*sit).second, _camPosition[(*sit).first], tree ); 
		values[_silhouetteCounter] = (bool*) calloc (_edgesData.size(), sizeof(bool));
		extractASilhouette(faceNormalsGPU, facePointsGPU, edgeFacesGPU, silhouetteValueGPU, _silhouetteCounter, silhouette, graph);
        _silhouetteCounter++;
	}

	time_t end = clock();


	cudaFree(faceNormalsGPU);
	cudaFree(facePointsGPU);
	cudaFree(edgeFacesGPU);
	cudaFree(silhouetteValueGPU);

	extractionTime = (end - start)/(float)(CLOCKS_PER_SEC);

	qDebug() << "Extracted " << _silhouetteCounter << " ASilhouettes - Elapsed Time " << extractionTime;
	qDebug() << "Extraction time: " << (float) silhouette/CLOCKS_PER_SEC << "sec";
	qDebug() << "Graph time: " << (float) graph/CLOCKS_PER_SEC << "sec";

	//Update Label
	QString txt("Captured:");
	if ( _silhouetteCounter < 10 ) txt += QString(" ");
	txt += QString::number( _silhouetteCounter );
	_silhouetteLabelGPU->setText( txt );

	//restore initial situation
	_camera.setObserver( base );
	_active = active;
	emit updateView();
}

void MotionSegmenter::extractASilhouette(float3* faceNormalsGPU, float3* facePointsGPU, int2* edgeFacesGPU, bool* silhouetteValueGPU, int index, clock_t& silhouette, clock_t& graph) {

	//CUDA kernel definition
	extern void computeSilhouetteCUDA( int faceNumber, float3* faceNormals, float3* facePoints, int edgeNumber, int2* edgeFaces, bool* output, float3 observer );

	//qDebug() << "CUDA silhouette";

	if ( _meshlist.empty() ) return; 
	//init edges data
	if ( _edgesData.empty() ) initEdges(*_meshlist[_active]);
	//collect face normals
	if ( faceNormals.empty() ) collectCudaInformations();

	//qDebug() << "All data collected!";

	clock_t start = clock();

	size_t faceNormalsSize = _meshlist[_active]->face.size() * sizeof(float3);
	size_t facePointsSize = _meshlist[_active]->face.size() * sizeof(float3);
	size_t edgeFacesSize = _edgesData.size() * sizeof(int2);
	size_t silhouetteValueSize = _edgesData.size() * sizeof(bool);

	//now start with copy the values onto the GPU memory
	cudaMemcpy( faceNormalsGPU, faceNormals[_active], faceNormalsSize, cudaMemcpyHostToDevice );
	cudaMemcpy( facePointsGPU,	facePoints[_active], facePointsSize, cudaMemcpyHostToDevice );
	cudaMemcpy( edgeFacesGPU, edgeFaces, edgeFacesSize, cudaMemcpyHostToDevice );

	//qDebug() << "Copy and allocation ok";

	//get the observer position
	float3 observer; 
	observer.x = _camera.getObserverX();
	observer.y = _camera.getObserverY();
	observer.z = _camera.getObserverZ();

	//call the kernel
	computeSilhouetteCUDA( _meshlist[_active]->face.size(), faceNormalsGPU, facePointsGPU, _edgesData.size(), edgeFacesGPU, silhouetteValueGPU, observer);

	//write data back to memory
	cudaMemcpy( values[index], silhouetteValueGPU, silhouetteValueSize, cudaMemcpyDeviceToHost );

	clock_t end = clock();

	silhouette += (end - start);

	start = clock();

	//creating silhouette
	for ( int i = 0; i < _edgesData.size(); i ++ ) {
		if ( values[index][i] ) {
			_silhouetteVector[index]->addVertex( _edgesData[i].v0 );
			_silhouetteVector[index]->addVertex( _edgesData[i].v1 );
			_silhouetteVector[index]->addEdge( _edgesData[i].v0, _edgesData[i].v1, i );
		}
	}
	_silhouetteVector[index]->finalize();
	
	end = clock();

	graph += (end - start);
}

void MotionSegmenter::collectCudaInformations() {

	//get space
	faceNormals.resize( _meshlist.size() );
	facePoints.resize( _meshlist.size() );

	for ( int i = 0; i < _meshlist.size(); i ++ ) {
		//allocate
		faceNormals[i] = (float3*) calloc ( _meshlist[i]->fn, sizeof(float3) );
		facePoints[i] = (float3*) calloc ( _meshlist[i]->fn, sizeof(float3) );
		//store normal information
		//assume that normals are already computed
		for ( int f = 0; f < _meshlist[i]->face.size(); f ++ ) {
			faceNormals[i][f].x = _meshlist[i]->face[f].N().X();
			faceNormals[i][f].y = _meshlist[i]->face[f].N().Y();
			faceNormals[i][f].z = _meshlist[i]->face[f].N().Z();
			//store information about a vertex
			facePoints[i][f].x = _meshlist[i]->face[f].P(0).X();
			facePoints[i][f].y = _meshlist[i]->face[f].P(0).Y();
			facePoints[i][f].z = _meshlist[i]->face[f].P(0).Z();
		}
	}

	edgeFaces = (int2*) calloc ( _edgesData.size(), sizeof(int2) );

	//collect data about edge adjacency
	for ( int i = 0; i < _edgesData.size(); i ++ ) {
		edgeFaces[i].x = _edgesData[i].f0;
		edgeFaces[i].y = _edgesData[i].f1;
	}

	//create space for boolean
	silhouetteValue = (bool*) calloc ( _edgesData.size(), sizeof(bool) );
	std::fill( silhouetteValue, silhouetteValue + (int) _edgesData.size(), false );		//no silhouette edges (probably this can be safely remove)
}

void MotionSegmenter::storeCluster(QColor current) {
	//check if there's something in the cluster!
	if ( clusterCandidate.empty() ) return;

	//create color and store it
	clusterColors.push_back(current);

	int clusterIndex = clusterList.size();

	//clear the candidateCluster
	for ( std::set<int>::iterator it = clusterCandidate.begin(); it != clusterCandidate.end(); it ++ ) {
		_meshlist[_active]->vert[*it].C() = vcg::Color4b::Blue;
		vertexToCluster[*it] = clusterIndex;
	}


	//store the cluster
	clusterList.push_back( clusterCandidate );
	//clear the cluster we just store
	clusterCandidate.clear();
	
	//refinement step!
	finalizeCluster(clusterList.size()-1, NOTCLUSTERED);
	finalizeCluster(NOTCLUSTERED, clusterList.size()-1);

	for ( std::set<int>::iterator it = clusterList.back().begin(); it !=  clusterList.back().end(); it ++ ) {
		_meshlist[_active]->vert[*it].C() = vcg::Color4b(current.red(), current.green(), current.blue(), 255);
	}

	//update view
	emit updateView();
}



void MotionSegmenter::finalizeCluster(int clusterA, int clusterB) {

	//get connected component of last cluster and unclustered elements

	qDebug() << "Cluster A: " << clusterA;

	//unmark all elements
	vcg::tri::UnMarkAll( *_meshlist[_active] );

        std::vector<std::set<int> > components;

	//first test: find connected component of A
	for (int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
		if (vertexToCluster[i] != clusterA) continue;
		if ( vcg::tri::IsMarked( *_meshlist[_active], &_meshlist[_active]->vert[i] ) ) continue;

		//else...
		std::set<int> connectedComponent;
		std::vector<int> stack;

		stack.push_back(i);

		while ( !stack.empty() ) {
			int current = stack.back(); stack.pop_back();
			vcg::tri::Mark( *_meshlist[_active], &_meshlist[_active]->vert[current] );
			connectedComponent.insert(current);
			//scorre i vicini usando meshGraph
			lemon::ListGraph::Node n = meshGraph.nodeFromId(current);
			
			for (lemon::ListGraph::IncEdgeIt e(meshGraph, n); e != lemon::INVALID; ++e) {
				//salva nello stack quelli che appartengono a clusterA e NON sono stati ancora marcati
				lemon::ListGraph::Node u = meshGraph.u(e);
				lemon::ListGraph::Node v = meshGraph.v(e);

				if ( u != n ) {
					int next = meshGraph.id(u);
					if ( vertexToCluster[next] == clusterA && !vcg::tri::IsMarked( *_meshlist[_active], &_meshlist[_active]->vert[next] ) )
						stack.push_back(next);
				}
				else {
					int next = meshGraph.id(v);
					if ( vertexToCluster[next] == clusterA && !vcg::tri::IsMarked( *_meshlist[_active], &_meshlist[_active]->vert[next] ) )
						stack.push_back(next);
				}
			}

		}

		components.push_back(connectedComponent);
	}

	//DEBUG
	qDebug() << "CC: " << components.size();

	for ( int i = 0; i < components.size(); i ++ ) 
		qDebug() << i << " Size: " << components[i].size();


	int maxc = 0;
	//search for the maximal component 
	for ( int i = 1; i < components.size(); i ++ ) 
		if( components[maxc].size() < components[i].size() ) maxc = i;

	if (components.size() > 1 ) {
		for ( int i = 0; i < components.size(); i ++ ) {
			if ( i == maxc ) continue;
			if ( components[i].size() > SIZE_LIMIT ) continue;

			for ( std::set<int>::iterator it = components[i].begin(); it != components[i].end(); ++it ) {
				vertexToCluster[*it] = clusterB;
				if ( clusterB != NOTCLUSTERED ) clusterList[clusterB].insert(*it);
				if ( clusterA != NOTCLUSTERED ) clusterList[clusterA].erase(*it);
			}
		}
	}

	//garbage collector!
	for (int i = 0; i < components.size(); i++) components[i].clear();
	components.clear();

}


/**
 * Edge length is computed as average of length
 **/
void MotionSegmenter::updateEdgeLengths( ) {

	for( int i = 0; i < _edgesData.size(); i ++ ) {
		//set length to zero
		_edgesData[i].length = 0;

		for ( int j = 0; j < _meshlist.size(); j ++ ) {
			_edgesData[i].length += vcg::Distance<double> ( _meshlist[j]->vert[_edgesData[i].v0].P(), _meshlist[j]->vert[_edgesData[i].v1].P() );
		}

		_edgesData[i].length /= _meshlist.size();
	}

}




void MotionSegmenter::secondStep() {

	qDebug() << "A.Silhouette Analysis...";

	//saliency vector
	saliency = new Eigen::SparseVector<int>(_meshlist[_active]->vert.size());

	clock_t start = clock();
	
	clock_t startUpdating;
	clock_t endUpdating;

	//1st stp: segmentation
	for ( int i = 0; i < _silhouetteVector.size(); i ++ ) {
		startUpdating = clock();
		_silhouetteVector[i]->computeSaliency( QString("adaptive"), _quality, *saliency);
		endUpdating = clock();
		analysisTime += ( endUpdating - startUpdating )/(float)CLOCKS_PER_SEC;
        delete _silhouetteVector[i];
	}
	//end of the insert operations
	saliency->finalize();

	clock_t end = clock();

	qDebug() << "Segmented " << _silhouetteVector.size() << " silhouettes in " << (float)(end-start)/CLOCKS_PER_SEC << "sec";
	qDebug() << "Analysis time: " << analysisTime << "sec";

	//activate clustering button%
	_dataCollect->setEnabled(false);
    _clusterDataButton->setEnabled(true);

	//prepare clustering (there's no clustered vertex)
	vertexToCluster.resize( _meshlist[_active]->vert.size() );
	std::fill( vertexToCluster.begin(), vertexToCluster.end(), NOTCLUSTERED );
	
	//clear all lists
	clusterList.clear();
	clusterCandidate.clear();
	clusterColors.clear();

	distanceFromSalient.clear();
	distanceFromSalient.resize( _meshlist[_active]->vert.size() );
	std::fill( distanceFromSalient.begin(), distanceFromSalient.end(), 0.0f );

	firstcluster = true;

	emit updateView();
	return;
}

void MotionSegmenter::firstCluster(Eigen::SparseVector<int>& saliency, int tshold) {

	std::set<int> significant;

	mask = (bool*) calloc ( _meshlist[_active]->vert.size(), sizeof(bool) );
    memset( mask, true, _meshlist[_active]->vert.size()*sizeof(bool) );
    //std::fill( mask, mask + _meshlist[_active]->vert.size(), true );

	clock_t start = clock();

	for (Eigen::SparseVector<int>::InnerIterator it(saliency); it; ++it) {
		  if ( it.value() > tshold ) {
			 vertexToCluster[it.index()] = 0;
			 significant.insert( it.index() );
			 //mask[it.index()] = true;
		  }
	}
	clock_t end = clock();

	clusteringTime += (float)(end-start)/CLOCKS_PER_SEC;
	qDebug() << "Significant: " << significant.size();
	qDebug() << "Not Significant: " << _meshlist[_active]->vert.size() - significant.size();

	/*********** FIRST CLUSTER COMPUTATION ***********/
	start = clock();
		
	//t = 125.0f;	//good for armadillo
	t = 0.2f;		//good for horse and elephant
    //t = 0.02f;	//good for cat

	float area = 0.0f;
	for ( int i = 0; i < _meshlist[_active]->face.size(); i ++ )
		area += vcg::DoubleArea( _meshlist[_active]->face[i] );
	area /= (2.0f * _meshlist[_active]->face.size());

	qDebug() << "Mean area: " << area; 

	nEvals = 0;

	for ( int i = 0; exp( -abs(evals[i])*t ) >= 0.1; nEvals ++, i ++ );

	qDebug() << "N Evals: " << nEvals;

	if (nEvals > eval_size) nEvals = eval_size;
	//we used only 20 evals for cat and horse
	//nEvals = 20;

    for ( int i = 0; i < eval_size; i ++ )
        evals[i] = expf( - 2.0f * fabs(evals[i]) * t );

	//allocate space on GPU
	prepareCUDADistance();

	//update quality and distanceFromSalient
	for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
		if ( vertexToCluster[i] == NOTCLUSTERED ) {
			continue;		//not-clustered vertex, skip
		}
        getDistanceFromVertex(i, nEvals);
	}

	//get the data back from the GPU
	cudaError_t debug = cudaMemcpy( distance, outputGPU, _meshlist[_active]->vert.size()*sizeof(float), cudaMemcpyDeviceToHost );

	for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
		_meshlist[_active]->vert[i].Q() = distanceFromSalient[i] = distance[i];
	}

	end = clock();
	qDebug() << "t " << t << "n.evals: " << nEvals << " CUDA Timing: " << (float) (end-start)/CLOCKS_PER_SEC;
	
	/*****COLOR DEBUG*****/
    std::fstream output("../output/distancedebug.txt", fstream::out );
	for ( int j = 0; j < _meshlist.size(); j ++ ) {
		for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
			_meshlist[j]->vert[i].Q() = distanceFromSalient[i] = distance[i];
			if ( j == _active ) output <<  distanceFromSalient[i] << std::endl;
		}
		if ( j == _active ) {
			minmax = vcg::tri::Stat<CGMesh>::ComputePerVertexQualityMinMax(*_meshlist[_active]);
			qDebug() << "Min def: " << minmax.first << " Max def: " << minmax.second;
		}
		vcg::tri::UpdateColor<CGMesh>::VertexQualityRamp( *_meshlist[j] );
	}
	output.close();

	//add dummy cluster and its color
	clusterList.push_back(significant);		
	QColor dummyColor = Qt::red;
	clusterColors.push_back(dummyColor);
	//colorize using distance from border
	vcg::tri::UpdateColor<CGMesh>::VertexQualityRamp( *_meshlist[_active] );
	//color clustered
	for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
		if ( vertexToCluster[i] != NOTCLUSTERED ) {
			_meshlist[_active]->vert[i].C() = vcg::Color4b( clusterColors[0].red(), clusterColors[0].green(), clusterColors[0].blue(), 255.0f );
		}
	}
}

void MotionSegmenter::getDistanceFromVertex(int vi, int nEvals) {

	//CUDA Kernel, see .cu files
	extern void computeDDistanceCUDA( float* evecsGPU, bool* mask, float* outputGPU, int nVert, int sizeEvals, int nEvals, int src );


	if ( vi < 0 || vi >= _meshlist[_active]->vert.size() ) return;

    computeDDistanceCUDA( evecsGPU, maskGPU, outputGPU, _meshlist[_active]->vert.size(), eval_size, nEvals, vi );

	return;
}

void MotionSegmenter::thirdStep() {

    qDebug() << "Clustering step 2.0";

    //first: get the seed
    int currentSeed;
    clock_t start, end;

    start = clock();

    if ( firstcluster ) {

        //allocate space
        eval_size = 200;
        evals = (float*) calloc (eval_size, sizeof(float));
        evecs = (float*) calloc ( _meshlist[_active]->vert.size() * eval_size, sizeof(float) );

        //read evals from file
        QString evalsfile = QFileDialog::getOpenFileName( NULL, "Open evals file", "", "txt (*.txt)");
        fstream in_evals( evalsfile.toAscii(), std::ios::in );


        for ( int i = 0; i < eval_size; i ++ ) {
            in_evals >> evals[i];	//i-simo eigenvalue
        }
        in_evals.close();

        //read evecs from file
        QString evecsfile = QFileDialog::getOpenFileName( NULL, "Open evecs file", "", "txt (*.txt)");
        fstream in_evecs( evecsfile.toAscii(), std::ios::in );

        for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
            for ( int j = 0; j < eval_size; j ++ ) {
                in_evecs >> evecs[i*eval_size + j];	//i riga, j colonna
            }
        }
        in_evecs.close();

        qDebug() << "Input data ok...";

        qDebug() << "Evals and Evecs correctly read";

        //identify significant vertices using adaptive clustering
        start = clock();
        firstCluster(*saliency, _silhouetteVector.size()/7);
        end = clock();

        clusteringTime += (float) (end-start)/CLOCKS_PER_SEC;

        qDebug() << "First cluster compututation: " << (float) (end-start)/CLOCKS_PER_SEC << "endl";

        _silhouetteVector.clear();
        currentSeed = std::max_element(distanceFromSalient.begin(), distanceFromSalient.end()) - distanceFromSalient.begin();
        firstcluster = false;
    }
    else
        currentSeed = getNextSeed( distanceFromSalient );

    //good for horse
    ts = 0.30f * minmax.second;	//for horse

    //still to define good threshold for cat, elephant, armadillo

    //ts = 0.005f * minmax.second;

    //ts = 0.3f * distanceFromSalient[currentSeed];


    qDebug() << "Threshold: " << ts;

    if ( distanceFromSalient[currentSeed] < ts ) {
        finalize();
        return;
    }

    std::set<int> dummyCluster;
    clusterList.push_back(dummyCluster);

    //we need a queue for the vertices
    std::vector<int> queue;
    queue.push_back( currentSeed );		//first element of the queue: currentSeed

    while (!queue.empty()) {
        //pop from the queue
        int current = queue.back();
        queue.pop_back();

        //element is already clustered: skip
        if ( vertexToCluster[current] != NOTCLUSTERED ) continue;

        if ( distanceFromSalient[current] <= ts ) {
            //find all the clustered neighbors and put them into the boundary set
            lemon::ListGraph::Node n = meshGraph.nodeFromId(current);
            //get the incident edges
            for (lemon::ListGraph::IncEdgeIt e(meshGraph, n); e != lemon::INVALID; ++e) {
                lemon::ListGraph::Node u = meshGraph.u(e);
                lemon::ListGraph::Node v = meshGraph.v(e);
                if ( vertexToCluster[meshGraph.id(u)] == clusterList.size()-1 ) {
                    boundaryVertices.insert( meshGraph.id(u) );
                }
                if ( vertexToCluster[meshGraph.id(v)] == clusterList.size()-1 ) {
                    boundaryVertices.insert( meshGraph.id(v) );
                }
            }
            continue;
        }

        //the measure is smaller than the threshold: add the element to the cluster
        clusterList.back().insert( current );
        vertexToCluster[current] = clusterList.size()-1;
        _meshlist[_active]->vert[current].C() = vcg::Color4b::LightGray;

        //insert the 1-ring neighbor of current vertex into the queue using mesh graph
        lemon::ListGraph::Node n = meshGraph.nodeFromId(current);
        //get the incident edges
        for (lemon::ListGraph::IncEdgeIt e(meshGraph, n); e != lemon::INVALID; ++e) {
            lemon::ListGraph::Node u = meshGraph.u(e);
            lemon::ListGraph::Node v = meshGraph.v(e);
            if ( u != n ) {
                    queue.push_back( meshGraph.id(u) );
            }
            else {
                queue.push_back( meshGraph.id(v) );
            }
        }
    }

    end = clock();

    clusteringTime += ( (float) (end-start)/CLOCKS_PER_SEC );


    //ulteriore migliorìa possibile: chiamare direttamente qua il getClosestCluster, in modo da
    //finalizzare in un sol botto

    emit updateView();
    emit showColorPicker();
}

/**
 * Get next seed using as measure the distanceFromSalient multibly by the overall distance from the other seeds.
 **/
int MotionSegmenter::getNextSeed(std::vector<float>& distanceFromSalient) {

	int candidate = -1;
	float maxdist = -10.0f;

	for ( int i = 0; i < distanceFromSalient.size(); i ++ ) {
		if ( vertexToCluster[i] != NOTCLUSTERED ) continue;	//esamina SOLO vertici non clusterizzati

		if ( distanceFromSalient[i] > maxdist ) {
			candidate = i;
			maxdist = distanceFromSalient[i];
		}
	}

	qDebug() << "Candidate DFS: " << distanceFromSalient[candidate];

	return candidate;
}

void MotionSegmenter::colorLastCluster(QColor current) {

	//update distance from salient for clustered vertices (can be done better)
	for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
		if ( vertexToCluster[i] != NOTCLUSTERED ) {
			_meshlist[_active]->vert[i].Q()  = distanceFromSalient[i] = 0;
			continue;		//clustered vertex, skip
		}
	}

	clusterColors.push_back(current);


	for ( std::set<int>::iterator s = boundaryVertices.begin(); s != boundaryVertices.end(); ++ s ) {
		getDistanceFromVertex(*s, nEvals);
	}

	//get the data back from the GPU
	cudaError_t debug = cudaMemcpy( distance, outputGPU, _meshlist[_active]->vert.size()*sizeof(float), cudaMemcpyDeviceToHost );
	
	for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
		_meshlist[_active]->vert[i].Q() = distanceFromSalient[i] = distance[i];
	}


	//prima colora sulla base della vertex quality
	vcg::tri::UpdateColor<CGMesh>::VertexQualityRamp( *_meshlist[_active] );

	for ( int i = 0; i < clusterList.size(); i ++ )  {
		for ( std::set<int>::iterator it = clusterList[i].begin(); it != clusterList[i].end(); ++ it ) {
			_meshlist[_active]->vert[*it].C() = vcg::Color4b( clusterColors[i].red(), clusterColors[i].green(), clusterColors[i].blue(), 255 );
		}
	}

	emit updateView();
}

void MotionSegmenter::finalize() {

	extern void computeClosestClusterCUDA( float* evecsGPU, bool* mask, float* outputDistanceGPU, int* outputIndicesGPU, 
										   int nVert, int sizeEvals, int nEvals, int src, int cluster );

	qDebug() << "Finalizing";

	clock_t start = clock();

	//set clusterIndices
	int *clusterIndices = (int*) calloc ( _meshlist[_active]->vert.size(), sizeof(int) );
	std::fill( clusterIndices, clusterIndices + _meshlist[_active]->vert.size(), 0 );

	int* clusterIndicesGPU;
	cudaError_t debug = cudaMalloc((void**) &clusterIndicesGPU, _meshlist[_active]->vert.size()*sizeof(int) );
	debug = cudaMemcpy( clusterIndicesGPU, clusterIndices, _meshlist[_active]->vert.size()*sizeof(int), cudaMemcpyHostToDevice );

	//reset distance value to FLT_MAX
	std::fill( distance, distance + _meshlist[_active]->vert.size(), FLT_MAX );
	debug = cudaMemcpy( outputGPU, distance, _meshlist[_active]->vert.size()*sizeof(float), cudaMemcpyHostToDevice );

	for ( std::set<int>::iterator it = boundaryVertices.begin(); it != boundaryVertices.end(); ++it ) {
		if ( vertexToCluster[*it] <= 0 ) continue;	//avoid using aggregate cluster informations
		//update distance from *it
		computeClosestClusterCUDA( evecsGPU, maskGPU, outputGPU, clusterIndicesGPU, _meshlist[_active]->vert.size(), eval_size, 50, *it, vertexToCluster[*it] );
	}

	//read data back from GPU
	debug = cudaMemcpy( distance, outputGPU, _meshlist[_active]->vert.size()*sizeof(float), cudaMemcpyDeviceToHost );
	debug = cudaMemcpy( clusterIndices, clusterIndicesGPU, _meshlist[_active]->vert.size()*sizeof(int), cudaMemcpyDeviceToHost );


	for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
		if ( vertexToCluster[i] > 0 ) continue;

		vertexToCluster[i] = clusterIndices[i];
		clusterList[clusterIndices[i]].insert(i);
	}

	clusterList[0].clear();
	
	clock_t end = clock();

	for ( int i = 0; i < clusterList.size(); i ++ )  {

		for ( std::set<int>::iterator it = clusterList[i].begin(); it != clusterList[i].end(); ++ it ) {
		
			//_meshlist[_active]->vert[*it].C() = vcg::Color4b( clusterColors[i].red(), clusterColors[i].green(), clusterColors[i].blue(), 255 );
			_meshlist[_active]->vert[*it].C() = vcg::Color4b::Scatter(clusterList.size(), i);
		}
	}

	clusteringTime += (float) (end-start)/CLOCKS_PER_SEC;

	qDebug() << "Recap:";
	qDebug() << "Extraction time: " << extractionTime << " sec";
	qDebug() << "Analysis time: " <<  analysisTime << " sec";
	qDebug() << "Clustering time: " << clusteringTime << " sec";

	clearCUDADistance();

    //Remove the dummy cluster
    for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
        vertexToCluster[i]--;
    }
    clusterList.erase( clusterList.begin() );

	emit updateView();

}



void MotionSegmenter::undoLastCluster() {
	qDebug() << "UNDO LAST";

	for ( std::set<int>::iterator it = clusterList.back().begin(); it != clusterList.back().end(); ++ it ) {
		vertexToCluster[*it] = NOTCLUSTERED;
		_meshlist[_active]->vert[*it].Q() = distanceFromSalient[*it];
	}

	clusterList.pop_back();

	//prima colora sulla base della vertex quality
	vcg::tri::UpdateColor<CGMesh>::VertexQualityRamp( *_meshlist[_active] );

	for ( int i = 0; i < clusterList.size(); i ++ )  {
		for ( std::set<int>::iterator it = clusterList[i].begin(); it != clusterList[i].end(); ++ it ) {
			_meshlist[_active]->vert[*it].C() = vcg::Color4b( clusterColors[i].red(), clusterColors[i].green(), clusterColors[i].blue(), 255 );
		}
	}

	emit updateView();
}


void MotionSegmenter::splitLastCluster() {

	for ( std::set<int>::iterator it = clusterList.back().begin(); it != clusterList.back().end(); ++ it ) {
		vertexToCluster[*it] = 0;
		clusterList[0].insert(*it);
		_meshlist[_active]->vert[*it].Q() = 0;
	}

	clusterList.pop_back();

	//ricalcola 
	for ( int i = 0; i < _meshlist[_active]->vert.size(); i ++ ) {
		if ( vertexToCluster[i] != NOTCLUSTERED ) continue;
		_meshlist[_active]->vert[i].Q() = distanceFromSalient[i];
	}

	//prima colora sulla base della vertex quality
	vcg::tri::UpdateColor<CGMesh>::VertexQualityRamp( *_meshlist[_active] );

	for ( int i = 0; i < clusterList.size(); i ++ )  {
		for ( std::set<int>::iterator it = clusterList[i].begin(); it != clusterList[i].end(); ++ it ) {
			_meshlist[_active]->vert[*it].C() = vcg::Color4b( clusterColors[i].red(), clusterColors[i].green(), clusterColors[i].blue(), 255 );
		}
	}

	emit updateView();
}

void MotionSegmenter::prepareCUDADistance() {
	extern void prepareCUDA ( cudaArray* evals_array, cudaChannelFormatDesc& evals_channel, float scaleFactor );

	qDebug() << "Starting cuda arrays...";

	cudaError_t debug;
	
	//1st: load eigenvalues as cudaArray
	cudaChannelFormatDesc channelDescEvals = cudaCreateChannelDesc<float>();
	debug = cudaMallocArray( &evals_array, &channelDescEvals, eval_size, 1);
	debug = cudaMemcpyToArray( evals_array, 0, 0, evals, eval_size*sizeof(float), cudaMemcpyHostToDevice );
    qDebug() << "ok evals array (on cudaArray)";

	//bind texture and load symbol t
	prepareCUDA( evals_array, channelDescEvals, t );
	
	//2nd: load eigenvectors as global memory (too large for allocating on array, use global memory)
	debug = cudaMalloc( (void**)&evecsGPU, eval_size*_meshlist[_active]->vert.size()*sizeof(float) );
	debug = cudaMemcpy( evecsGPU, evecs, eval_size*_meshlist[_active]->vert.size()*sizeof(float), cudaMemcpyHostToDevice );
	qDebug() << "ok evecs array (on global memory)";

	//3rd boolean mask
	debug = cudaMalloc( (void**)&maskGPU, _meshlist[_active]->vert.size()*sizeof(bool) );
	debug = cudaMemcpy( maskGPU, mask, _meshlist[_active]->vert.size()*sizeof(bool), cudaMemcpyHostToDevice );
	qDebug() << "ok boolean mask (on global memory)";

	//4th: allocate empty space for output
	debug = cudaMalloc( (void**) &outputGPU, _meshlist[_active]->vert.size()*sizeof(float) );
	qDebug() << "ok output (on global memory)";

	//allocate output buffer;
	distance = (float*) calloc ( _meshlist[_active]->vert.size(), sizeof(float) );
    std::fill( distance, distance + _meshlist[_active]->vert.size(), FLT_MAX );
    debug = cudaMemcpy( outputGPU, distance, _meshlist[_active]->vert.size()*sizeof(float), cudaMemcpyHostToDevice );
    qDebug() << "ok output (on global memory)";
}

void MotionSegmenter::clearCUDADistance() {	//clean CUDA
	cudaFreeArray( evals_array );
	cudaFree( evecsGPU );
	cudaFree( maskGPU );
	cudaFree( outputGPU );
}


void MotionSegmenter::selectSilhouettes(int n) {
	silhouettePairs.clear();

	if ( n >= _camPosition.size() * _meshlist.size() ) 
		n = _camPosition.size() * _meshlist.size();

	qDebug() << "N: " << n;

	while ( silhouettePairs.size() < n ) {
		int s = rand()%(_camPosition.size());
		int p = rand()%(_meshlist.size());

		silhouettePairs.insert( std::pair<int,int>(s,p) );
	}
}


/**
 * Randomly select n viewpoints
 **/
void MotionSegmenter::selectViewPoints(int n) {
	//if ( n >= _camPosition.size() ) return;

	_camPosition.clear();
	initCamPosition();

	if ( n >= _camPosition.size() ) return;

	//store every element in tmp
	std::vector<CGPoint> tmp;
	tmp.insert(tmp.begin(), _camPosition.begin(), _camPosition.end());
	_camPosition.clear();

	for ( int i = 0; i < n; i ++ ) {
		
		int s = rand()%(tmp.size());
		_camPosition.push_back(tmp[s]);
		tmp.erase(tmp.begin()+s);
	}

	//update selection
	selectSilhouettes( _ksSpinBox->value() );
}
